/********************************************************************************
 * Copyright(c) 2020-2025 VINCENT_WY All rights reserved.
 * - Filename
 * - Author  Vincent
 * - Version V1.0.0
 * - Date    2023/09/21
 * - Brief
 * - FunctionList:
 ******************************************************************************
 * History:
 *
 *
 *
 ******************************************************************************
 */
#include "NoBufferedReplayImpl.hpp"
#include <fstream>
#include "log.h"
#define BASE_PORT 58000

using namespace std;
NoBufferedReplayImpl::NoBufferedReplayImpl(const int &id, const std::string &path)
    : TAG("NoBufferedReplayImpl-" + std::to_string(id)), _frameId(id), _root(path)
{
    LOGI(TAG, "read file ...");
    _fileReaderPtr = std::make_unique<DispatchFileReader>(_root, _frameId);
    _fileInfoList = _fileReaderPtr->getFileInfoList();
    LOGI(TAG, "Create ...");
}

NoBufferedReplayImpl::~NoBufferedReplayImpl()
{
    LOGI(TAG, "Destroye ...");
}

void NoBufferedReplayImpl::start(
    const std::function<void(std::shared_ptr<lpc::LidarProtoPointCloud> &&)> &callback, const long long int &time)
{
    if (_fileInfoList.empty())
    {
        LOGE(TAG, "NoFile in list !!!");
        return;
    }
    _startTime = time;
    _context= std::make_unique<message::Context>(TAG);
    _lpcPointCloudPub =_context->createPublisher<lpc::LidarProtoPointCloud>("tcp://*:"+std::to_string(BASE_PORT+_frameId));
    _readLoop = std::make_unique<std::thread>(&NoBufferedReplayImpl::ReadLoop, this);
    _invokeLoop =
        std::make_unique<std::thread>(&NoBufferedReplayImpl::InvokeLoop, this, callback);
    _status = Status::START;
}
bool NoBufferedReplayImpl::isComplete()
{
    return _isComplete;
}

uint64_t NoBufferedReplayImpl::getFirstFrameTm()
{
    if (_fileInfoList.empty())
        return 0;
    return _fileInfoList[0].timestamp;
}

void NoBufferedReplayImpl::doRepeat()
{
    _status = Status::REPEAT;
}

void NoBufferedReplayImpl::stop()
{
    LOGI(TAG, "Stop 1...");
    _status = Status::IDLE;
    _isExit = true;
    _frameCv.notify_all();
    if ((_readLoop != nullptr) && _readLoop->joinable())
    {
        _readLoop->join();
    }
    if ((_invokeLoop != nullptr) && _invokeLoop->joinable())
    {
        _invokeLoop->join();
    }
    LOGI(TAG, "Stop 2...");
}

void NoBufferedReplayImpl::ReadLoop()
{
    int index = 0;
    long long int mLastTimeStamp{};
    long long int needSendTime{};
    while (!_isExit)
    {
        switch (_status)
        {
        case Status::START:
        {
            index = 0;
            _status = Status::CONTINUE;
            mLastTimeStamp = _startTime;
            needSendTime = TimeUtil::now_us();
            _isComplete = false;
        }
        break;

        case Status::REPEAT:
        {
            _status = Status::START;
        }
        break;

        case Status::CONTINUE:
        {
            auto frameInterval = _fileInfoList[index].timestamp - mLastTimeStamp;
            unsigned long long t1{}, t2{}, readInterval{};
            long long int delayTime{};
            // if (frameInterval == 0)
            //     goto NEXT_FRAME;
            t1 = TimeUtil::now_us();
            auto frame = _fileReaderPtr->getDispatchFrame(_fileInfoList[index]);
            t2 = TimeUtil::now_us();
            // 等待两帧之间的时间间隔，时间是us
            readInterval = t2 - t1;

            needSendTime += frameInterval;
            delayTime = needSendTime - TimeUtil::now_us();
            LOG_I(TAG, "Replay get lidar {} frame timestamp {}, frameInterval {}, readInterval {}, delay {}",
                  _frameId, _fileInfoList[index].timestamp,
                  frameInterval, readInterval, delayTime);

            if (delayTime > 0)
            {
                std::this_thread::sleep_for(std::chrono::microseconds(delayTime));
            }

            {
                std::lock_guard<std::mutex> lock(_frameMtx);
                _frameDqu.push_back(frame);
            }
            _frameCv.notify_one();

            mLastTimeStamp = _fileInfoList[index].timestamp;
            // NEXT_FRAME:
            //     index++;
            //     while ((long long)readInterval - delayTime > _fileInfoList[index].timestamp - mLastTimeStamp && index < (int)_fileInfoList.size())
            //     {
            //         index++;
            //     }
            index++;
            if (index >= (int)_fileInfoList.size())
            {
                _status = Status::IDLE;
                _isComplete = true;
                index = 0;
                LOG_I(TAG, "lidar id [%d] complete.\n", _frameId);
            }
        }
        break;

        case Status::IDLE:
        {
            std::this_thread::sleep_for(std::chrono::milliseconds(3));
        }
        break;

        default:
            break;
        }
    }
}

void NoBufferedReplayImpl::InvokeLoop(
    const std::function<void(std::shared_ptr<lpc::LidarProtoPointCloud> &&)> &callbak)
{
    while (!_isExit)
    {
        std::unique_lock<std::mutex> lock(_frameMtx);
        _frameCv.wait(lock, [this]
                      { return _frameDqu.size() > 0 || _isExit; });
        if (_isExit)
            break;
        auto frame = _frameDqu.front();
        _frameDqu.pop_front();
        lock.unlock();
        // LOG_I(TAG, "Replay send camera {} frame timestamp {}", _frameId, frame->timestamp());
        if (callbak != nullptr)
        {
            callbak(std::move(frame));
        }
        if (TimeUtil::fps_calc_inc(_fps))
        {
            LOG_I(TAG, "[FPS] Replay lidar {} fps {:.02f}, total fps{:.02f}", _frameId, _fps.ins_fps, _fps.avg_fps);
        }
        auto start = TimeUtil::now_ms_double();
        _lpcPointCloudPub->write(frame);
        auto end = TimeUtil::now_ms_double();
    }
}